#include "rigid_body.h"

namespace gc {
namespace MPhys {
	const double e_collision_table[] = {
		0.1,        // Aluminum
		0.5,        // Wood
		0.55555556, // Iron
		0.69,       // Glass
		0.88888889, // Ivory
	};

	__host__ RigidBodyNoRot::RigidBodyNoRot(Float mass, PhysicalMaterial material)
		:mass(mass), material(material),
		position{ 0,0,0 }, v{ 0,0,0 }, a{0,-a_Gravity,0},
		a_force{0,0,0}, forceDuration(0)
	{
		e_collision = e_collision_table[material];
	}

	__host__ __device__ void RigidBodyNoRot::move(Float time_delta) {
		// update using velocity verlet
		Point3f a0 = a;
		if (forceDuration > 0) {
			forceDuration -= time_delta;
			if (forceDuration <= 0) a = a - a_force;
		}

		Point3f v_half_dt = v + a0*time_delta*0.5;  // step 1: v(t+0.5dt) = v(t) + 0.5*a(t)*dt
		position = position + v_half_dt * time_delta; // step 2: x(t+dt) = x(t) + v(t)*h + 0.5*a(t)*dt^2 = x(t) + v(t+0.5dt)*dt
		v = v + (a + a0) * (0.5 * time_delta);  // step 3: v(t+0.5dt) = v(t) + 0.5(a(t)+a(t+0.5dt))*dt
	}

	__host__ __device__ void RigidBodyNoRot::receiveMomentum(const Point3f &momentum) {
		v = momentum * (1. / mass) + v;
	}

	__host__ __device__ void RigidBodyNoRot::receiveVelocity(const Point3f &dv) {
		v = dv + v;
	}

	__host__ __device__ void RigidBodyNoRot::setForce(const Point3f &force, Float duration) {
		a_force = force * (1. / mass);
		a = a + a_force;
		forceDuration = duration;
	}

	__host__ __device__ void collide(const Point3f &n_ab, RigidBodyNoRot &a, RigidBodyNoRot &b) {
		Float u_an = dot(n_ab, a.v);
		Float u_bn = dot(n_ab, b.v);

		// assume a.e = b.e = COR<a,b>
		Float momentum_a = a.mass*u_an, momentum_b = b.mass*u_bn;
		Float invTotalMass = 1. / (a.mass + b.mass);
		Float v_an = (momentum_a + momentum_b + b.e_collision*(momentum_b - b.mass*u_an))*invTotalMass;
		Float v_bn = (momentum_a + momentum_b + b.e_collision*(momentum_a - a.mass*u_bn))*invTotalMass;

		a.v = a.v + n_ab * (v_an - u_an);
		b.v = b.v + n_ab * (v_bn - u_bn);
	}

}
}